/*
 * state_machine.h
 *
 *  Created on: Jan 11, 2013
 *      Author: michiel
 */

#ifndef STATE_MACHINE_H_
#define STATE_MACHINE_H_

#include <ros/ros.h>
#include <sensor_msgs/Joy.h>
#include <actionlib/client/simple_action_client.h>
#include <std_msgs/Float32.h>
#include <geometry_msgs/Twist.h>

#include "topic/topic.h"
#include "PS3Controller/PS3_buttons.h"
#include "comm/emotion_msg.h"

#include "rp_states/DriveAction.h"
#include "rp_states/DropBallAction.h"
#include "rp_states/FindMarkerAction.h"
#include "rp_states/PositionTargetAction.h"
#include "rp_states/IdleAction.h"
#include "rp_states/GrabBallAction.h"
#include "rp_states/ManualControlAction.h"


enum STATE {
	IDLE,
	MOVING_WITHOUT_BALL,
	EMOTING,
	POSITIONING,
	GRABBING,
	MOVING_WITH_BALL,
	RELEASING,
	SEARCHING,
	MANUAL,
};

typedef actionlib::SimpleActionClient<rp_states::DropBallAction>		BallDropper;
typedef actionlib::SimpleActionClient<rp_states::FindMarkerAction>		MarkerFinder;
typedef actionlib::SimpleActionClient<rp_states::PositionTargetAction>	Positioner;
typedef actionlib::SimpleActionClient<rp_states::GrabBallAction>		BallGrabber;
typedef actionlib::SimpleActionClient<rp_states::IdleAction>			Idler;
typedef actionlib::SimpleActionClient<rp_states::DriveAction>			Driver;
typedef actionlib::SimpleActionClient<rp_states::ManualControlAction>	Manual;

typedef sensor_msgs::Joy::_buttons_type									BUTTONS;
typedef sensor_msgs::Joy::_axes_type									AXES;
typedef actionlib::SimpleClientGoalState								GOALSTATE;

class StateMachine {
	/*
	 * VARIABLES.
	 */
private:
	STATE _currentState;
protected:
	ros::NodeHandle nh_;

	BallDropper			ballDropper_;
	MarkerFinder		markerFinder_;
	Positioner			positioner_;
	BallGrabber			ballGrabber_;
	Idler				idler_;
	Driver				driver_;
	Manual				manualController_;

	ros::Subscriber		wheels_sub_;
	ros::Subscriber		controller_sub_;
	ros::Publisher		emo_pub_;

	geometry_msgs::Twist wheels_speed_;
	double				wheels_turn_;

	bool				button_cancel_pressed_;
	bool				button_grab_pressed_;
	bool				button_drop_pressed_;
	bool				button_start_pressed_;
	bool				manual_mode_;

	unsigned int		marker_ball_;
	unsigned int		marker_basket_;
	unsigned int		marker_current_;

	rp_states::IdleResult			idle_result_;
	rp_states::DriveResult			drive_result_;
	rp_states::GrabBallResult		grab_result_;
	rp_states::DropBallResult		drop_result_;
	rp_states::PositionTargetResult	position_result_;
	rp_states::FindMarkerResult		find_result_;
	rp_states::ManualControlResult	manual_result_;

	bool idler_busy_;
	bool driver_busy_;
	bool grabber_busy_;
	bool dropper_busy_;
	bool positioner_busy_;
	bool finder_busy_;
	bool manual_busy_;

	ros::Time emoTime_;
	comm::emotion emomsg;
public:
	/*
	 * FUNCTIONS.
	 */
private:
protected:
	void prepareEmotion(int state, ros::Duration time);
	void wheelCallback(const geometry_msgs::Twist::ConstPtr &msg);
	void controllerCallback(const sensor_msgs::Joy::ConstPtr &msg);

	void goalCanceler();

	void idlerDoneCallback(const GOALSTATE &state, const rp_states::IdleResultConstPtr &result);
	void driverDoneCallback(const GOALSTATE &state, const rp_states::DriveResultConstPtr &result);
	void ballDropperDoneCallback(const GOALSTATE &state, const rp_states::DropBallResultConstPtr &result);
	void positionerDoneCallback(const GOALSTATE &state, const rp_states::PositionTargetResultConstPtr &result);
	void ballGrabberDoneCallback(const GOALSTATE &state, const rp_states::GrabBallResultConstPtr &result);
	void markerFinderDoneCallback(const GOALSTATE &state, const rp_states::FindMarkerResultConstPtr &result);
	void manualControlDoneCallback(const GOALSTATE &state, const rp_states::ManualControlResultConstPtr &result);

	void idlerActiveCallback();
	void driverActiveCallback();
	void ballDropperActiveCallback();
	void positionerActiveCallback();
	void ballGrabberActiveCallback();
	void markerFinderActiveCallback();
	void manualControlActiveCallback();
public:
	/*
	 * Make sure node names of the servers in the launch files are the
	 * same as the values provided here.
	 */
	StateMachine() : nh_(),
	    ballDropper_("drop_ball", true),
		markerFinder_("find_marker", true),
		positioner_("position_target", true),
		ballGrabber_("grab_ball", true),
		idler_("idle", true),
		driver_("drive", true),
		manualController_("manual_control", true)
	{
		idler_busy_			= false;
		driver_busy_		= false;
		grabber_busy_		= false;
		dropper_busy_		= false;
		positioner_busy_	= false;
		finder_busy_		= false;
		manual_busy_		= false;
		manual_mode_		= false;
		button_grab_pressed_ = false;
		button_cancel_pressed_ = false;
		button_drop_pressed_ = false;
		button_start_pressed_ = false;
		_currentState = IDLE;
		emoTime_ = ros::Time::now();
		emomsg.state = false;
		init();
	}
	~StateMachine() {
		nh_.shutdown();

		// Cancel all goals currently running on action servers.
		ballDropper_.cancelAllGoals();
		markerFinder_.cancelAllGoals();
		positioner_.cancelAllGoals();
		ballGrabber_.cancelAllGoals();
		idler_.cancelAllGoals();
		driver_.cancelAllGoals();
		manualController_.cancelAllGoals();
	}
	void init();
	void spin();

	void prepareBallDrop();
	void prepareFindMarker();
	void preparePositionTarget();
	void prepareGrabBall();
	void prepareIdle();
	void prepareDrive(bool has_ball);
	void prepareManual();

	void emotionHandler();
	void stateSwitcher();
	void stateHandler();

	STATE getState();
	void setState(STATE state);
	inline void setPositionerTarget(unsigned int marker);
	inline int getPositionerTarget();
};


#endif /* STATE_MACHINE_H_ */
